function qMF = dcm2quat_cg( CFM ) %#codegen
%DCM2QUAT_CG dcm2quat函数的代码生成用版本，从Aerospace工具箱复制而来

coder.extrinsic('error', 'message');

if any(~isreal(CFM) || ~isnumeric(CFM))
    error(message('aero:dcm2quat:isNotReal'));
end

if ((size(CFM,1) ~= 3) || (size(CFM,2) ~= 3))
    error(message('aero:dcm2quat:wrongDimension'));
end

qMF = coder.nullcopy(zeros(size(CFM,3), 4));

for i = size(CFM,3):-1:1

    qMF(i,4) =  0; 
    
    tr = trace(CFM(:,:,i));

    if (tr > 0)
        sqtrp1 = sqrt( tr + 1.0 );
        
        qMF(i,1) = 0.5*sqtrp1; 
        qMF(i,2) = (CFM(2, 3, i) - CFM(3, 2, i))/(2.0*sqtrp1);
        qMF(i,3) = (CFM(3, 1, i) - CFM(1, 3, i))/(2.0*sqtrp1); 
        qMF(i,4) = (CFM(1, 2, i) - CFM(2, 1, i))/(2.0*sqtrp1); 
    else
        d = diag(CFM(:,:,i));
        if ((d(2) > d(1)) && (d(2) > d(3)))
            % max value at dcm(2,2,i)
            sqdip1 = sqrt(d(2) - d(1) - d(3) + 1.0 );
            
            qMF(i,3) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            qMF(i,1) = (CFM(3, 1, i) - CFM(1, 3, i))*sqdip1; 
            qMF(i,2) = (CFM(1, 2, i) + CFM(2, 1, i))*sqdip1; 
            qMF(i,4) = (CFM(2, 3, i) + CFM(3, 2, i))*sqdip1; 
        elseif (d(3) > d(1))
            % max value at dcm(3,3,i)
            sqdip1 = sqrt(d(3) - d(1) - d(2) + 1.0 );
            
            qMF(i,4) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            qMF(i,1) = (CFM(1, 2, i) - CFM(2, 1, i))*sqdip1;
            qMF(i,2) = (CFM(3, 1, i) + CFM(1, 3, i))*sqdip1; 
            qMF(i,3) = (CFM(2, 3, i) + CFM(3, 2, i))*sqdip1; 
        else
            % max value at dcm(1,1,i)
            sqdip1 = sqrt(d(1) - d(2) - d(3) + 1.0 );
            
            qMF(i,2) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            qMF(i,1) = (CFM(2, 3, i) - CFM(3, 2, i))*sqdip1; 
            qMF(i,3) = (CFM(1, 2, i) + CFM(2, 1, i))*sqdip1; 
            qMF(i,4) = (CFM(3, 1, i) + CFM(1, 3, i))*sqdip1; 
        end
    end
end
end